#include <mc_core/utils/math_utils.h>

namespace math_utils{

/************************************
 * 角度、距离计算
 * https://wiki.ros.org/angles
 *************************************/

// 弧度转角度
double toDeg(double radians){
    return radians * 180.0 / M_PI;
}

// 角度转弧度
double toRad(double degrees){
    return degrees * M_PI / 180.0;
}

// 角度转换 [-PI,PI]
double normalizeAngle(double angle){
  const double result = fmod(angle + M_PI, 2.0 * M_PI);
  if (result <= 0.0)
    return result + M_PI;
  return result - M_PI;
}

// 角度差 b - a
double diffAngle(double a, double b){
    return normalizeAngle(b - a);
}

// 角度相加
double addAngle(double a, double b){
    return normalizeAngle(a + b);
}

// 计算两个点之间的角度
double angleBetweenPoses(geometry_msgs::Pose a, geometry_msgs::Pose b){
    double dx = b.position.x - a.position.x;
    double dy = b.position.y - a.position.y;
    return atan2(dy, dx);
}

// 计算两个点之间的距离
double distanceBetweenPoses(geometry_msgs::Pose a, geometry_msgs::Pose b){
    double dx = b.position.x - a.position.x;
    double dy = b.position.y - a.position.y;
    return hypot(dx, dy);
}


}; //math_utils 